﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Algorithm.Check;
using Common;
using static Algorithm.Check.ExtensionForCRC16;

namespace Device
{
    /// <summary>
    /// 电机运动模式
    /// </summary>
    public enum MotorModes
    {
        /// <summary>
        /// 速度模式
        /// </summary>
        speedMode,

        /// <summary>
        /// 扭矩模式
        /// </summary>
        torqueMode,

        /// <summary>
        /// 位置+转矩
        /// </summary>
        locAndtor
    }

    /// <summary>
    /// 国产电机LC10P类
    /// </summary>
    public class LC10P
    {

        private static readonly object locker= new object();
        private enum MotorSet
        {
            speed,
            reSpeed,
            torque,
            location,
            locSpeed,
            locMove,
            cmd_enable,
            torqueMove
        }

        private bool IsMonitorOK = true;
        private bool StopMonitor = false;

        /// <summary>
        /// 实时位置、速度、转矩
        /// </summary>
        public bool MonitorStatus { get; set; } = false;

        /// <summary>
        /// 实时位置
        /// </summary>
        public bool MonitorLocationStatus { get; set; } = false;

        /// <summary>
        /// 实时速度
        /// </summary>
        public bool MonitorSpeedStatus { get; set; } = false;

        /// <summary>
        /// 实时转矩
        /// </summary>
        public bool MonitorTorqueStatus { get; set; } = false;

        /// <summary>
        /// 速度
        /// </summary>
        public short Speed { get; set; }

        /// <summary>
        /// 扭矩
        /// </summary>
        public short Torque { get; set; }

        /// <summary>
        /// 位置
        /// </summary>
        public int Location { get; set; }

        private int waitTime = 50;
        private bool connected = false;
        private SerialPort serialPort;
        private object deviceName = "伺服驱动器";

        /// <summary>
        /// 电机模式
        /// </summary>
        public MotorModes motorMode { get; set; }

        private ManualResetEventSlim responseCompletedEvent = new ManualResetEventSlim(false);

        private byte[] recvData;

        /// <summary>
        /// 设备连接
        /// </summary>
        /// <param name="port"> 串口 </param>
        /// <param name="baud"> 波特率 </param>
        /// <exception cref="Exception"> </exception>
        [Conditional("DEVICE")]
        public void Connect(string port, int baud = 9600)
        {
            if (connected)
                return;
            serialPort = new SerialPort();
            serialPort.BaudRate = baud;
            serialPort.PortName = port;
            serialPort.Parity = Parity.Even;
            ManualResetEventSlim openCompletedEvent = new ManualResetEventSlim(false);
            Task.Run(() =>
            {
                serialPort.Open();
                openCompletedEvent.Set();
            });
            if (!openCompletedEvent.Wait(2000))
            {
                throw new Exception($"{deviceName}连接超时");
            }
            serialPort.DataReceived += SerialPort_DataReceived;

            connected = true;
        }

        /// <summary>
        /// 关闭连接
        /// </summary>
        public void Close()
        {
            connected = false;
            serialPort?.Close();
        }

        private void SerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            Thread.Sleep(100);
            var sp = sender as SerialPort;
            int bytesToRead = serialPort.BytesToRead;
            if (bytesToRead > 0)
            {
                recvData = new byte[bytesToRead];
                serialPort.Read(recvData, 0, bytesToRead);
                Mylog.Debug($"{deviceName}  [{BitConverter.ToString(recvData).Replace("-", " ")}]");
                responseCompletedEvent.Set();
            }
        }

        /// <summary>
        /// 对返回的数据做CRC校验
        /// </summary>
        /// <param name="data"> </param>
        /// <returns> </returns>
        private bool Check(byte[] data)
        {
            byte[] tmp = data.Take(data.Length - 2).ToArray();
            return data.SequenceEqual(tmp.WithCRC16(CRC16Type.Modbus).ToArray());
        }

        private byte[] SendCmdAndWaitResponse(byte[] cmd)
        {
            lock (locker)
            {


                if (serialPort is null)
                {
                    return null;
                }
                Mylog.Debug($"{deviceName} + Cmd[{BitConverter.ToString(cmd).Replace("-", " ")}]");
                recvData = null;
                responseCompletedEvent.Reset();
                serialPort.Write(cmd, 0, cmd.Length);

                if (!responseCompletedEvent.Wait(3000))
                {
                    throw new Exception($"{deviceName}通信超时");
                }
                return recvData;
            }
        }

        #region 伺服控制指令

        //电机工作模式
        private Dictionary<MotorModes, byte[]> cmd_motor = new Dictionary<MotorModes, byte[]>
        {
            { MotorModes.speedMode, new byte[] { 01, 06, 02, 00, 00, 00 } },
            { MotorModes.torqueMode, new byte[] { 01, 06, 02, 00, 00, 02 } },
            { MotorModes.locAndtor, new byte[] { 01, 06, 02, 00, 00, 05 } },
        };

        private static Dictionary<MotorSet, byte[]> cmd_motorSpeedMove = new Dictionary<
            MotorSet,
            byte[]
        >
        {
            { MotorSet.speed, new byte[] { 01, 06, 06, 03 } },
            //{ MotorSet.cmd_enable, new byte[] { 01, 06, 0x31, 00, 00, 01 } },
        };

        private static Dictionary<MotorSet, byte[]> cmd_motorTorqueMove = new Dictionary<
            MotorSet,
            byte[]
        >
        {
            { MotorSet.speed, new byte[] { 01, 06, 07, 19 } },
            { MotorSet.reSpeed, new byte[] { 01, 06, 07, 20 } },
            { MotorSet.torque, new byte[] { 01, 06, 07, 03 } },
            //{ MotorSet.cmd_enable, new byte[] { 01, 06, 0x31, 00, 00, 01 } },
            //{ MotorSet.torqueMove, new byte[] { 01, 06, 0x31, 00, 00, 01 } },
        };

        private static Dictionary<MotorSet, byte[]> cmd_motorLocTorMove = new Dictionary<
            MotorSet,
            byte[]
        >
        {
            { MotorSet.speed, new byte[] { 01, 06, 07, 19 } },
            { MotorSet.reSpeed, new byte[] { 01, 06, 07, 20 } },
            { MotorSet.torque, new byte[] { 01, 06, 07, 03 } },
            { MotorSet.locSpeed, new byte[] { 01, 06, 0x11, 14 } },
            { MotorSet.location, new byte[] { 01, 0x10, 0x11, 12, 00, 02, 04 } },
        };

        /// <summary>
        /// 选择电机模式指令Loc：位置，Tor：转矩，Speed：速度
        /// </summary>
        private static Dictionary<MotorModes, Dictionary<MotorSet, byte[]>> cmd_motorMove =
            new Dictionary<MotorModes, Dictionary<MotorSet, byte[]>>
            {
                { MotorModes.speedMode, cmd_motorSpeedMove },
                { MotorModes.torqueMode, cmd_motorTorqueMove },
                { MotorModes.locAndtor, cmd_motorLocTorMove },
            };

        /// <summary>
        /// 电机使能
        /// </summary>
        private byte[] cmd_motor_enable = { 01, 06, 0x31, 00, 00, 01 };

        /// <summary>
        /// 位置混合模式下的位置使能准备
        /// </summary>
        private byte[] cmd_motor_locEnable = { 01, 06, 0x31, 00, 00, 03 };

        /// <summary>
        /// 位置混合模式下的位置使能动作
        /// </summary>
        private byte[] cmd_motor_locMove = { 01, 06, 0x31, 00, 00, 07 };

        /// <summary>
        /// 断使能
        /// </summary>
        private byte[] cmd_motor_disable = { 01, 06, 0x31, 00, 00, 00 };

        // 读取扭矩参数命令
        private byte[] cmd_read_torque = { 01, 03, 0x0b, 02, 00, 01 };

        private byte[] cmd_set_origin = { 01, 06, 05, 30, 00, 06 };
        private byte[] cmd_to_origin = { 01, 06, 05, 30, 00, 05 };

        //读取速度
        private byte[] cmd_read_speed = { 01, 03, 0x0b, 00, 00, 02 };

        //读取位置
        private byte[] cmd_read_location = { 01, 03, 0x0b, 07, 00, 02 };

        #endregion 伺服控制指令

        #region 伺服运动指令

        /// <summary>
        /// 设置转速为0
        /// </summary>
        public void Stop()
        {
            try
            {
                switch (motorMode)
                {
                    case MotorModes.speedMode:
                        MotorSpeed(0);
                        break;

                    case MotorModes.torqueMode:
                        MotorTorqueSetAndMove(0);
                        break;

                    case MotorModes.locAndtor:
                        MotorTorqueSetAndMove(0);
                        break;

                    default:
                        break;
                }
            }
            catch (Exception)
            {
                throw new Exception("停止出现异常");
            }
        }

        /// <summary>
        /// 初始化
        /// </summary>
        public void Init()
        {
            MotorSendCommand(cmd_motor_disable.WithCRC16(CRC16Type.Modbus).ToArray()); //断使能
            Thread.Sleep(100);
            MotorSendCommand(cmd_motor[motorMode].WithCRC16(CRC16Type.Modbus).ToArray()); //设置模式
            switch (motorMode)
            {
                case MotorModes.speedMode:
                    MotorSendCommand(cmd_motor_enable.WithCRC16(CRC16Type.Modbus).ToArray()); //上速度使能
                    break;

                case MotorModes.torqueMode:
                    MotorSendCommand(cmd_motor_enable.WithCRC16(CRC16Type.Modbus).ToArray()); //上转矩使能
                    break;

                case MotorModes.locAndtor:
                    MotorSendCommand(cmd_motor_locEnable.WithCRC16(CRC16Type.Modbus).ToArray()); //上位置使能
                    break;

                default:
                    break;
            }
            //byte[] bytes = new byte[0];
            //CmdCopy(bytes, MotorSet.cmd_enable);
        }

        /// <summary>
        /// 速度模式
        /// </summary>
        /// <param name="speed"> 速度范围-6000~6000 </param>
        public void MotorSpeed(int speed)
        {
            byte[] bSpeed = BitConverter.GetBytes(speed).Reverse().Skip(2).ToArray();
            CmdCopy(bSpeed, MotorSet.speed);
        }

        /// <summary>
        /// 设置转矩模式下速度
        /// </summary>
        /// <param name="speed1">  速度范围0~6000 </param>
        /// <param name="isZheng"> true时设置正转速度，false设置反转速度 </param>
        public void MotorTorqueSpeed(int speed1, bool isZheng)
        {
            byte[] bSpeed = BitConverter.GetBytes(speed1).Reverse().Skip(2).ToArray();
            if (!isZheng)
            {
                CmdCopy(bSpeed, MotorSet.reSpeed);
            }
            else
            {
                CmdCopy(bSpeed, MotorSet.speed);
            }
        }

        /// <summary>
        /// 设置转矩并启动，转矩为负是反转
        /// </summary>
        /// <param name="torque"> 转矩范围-3000~3000 </param>
        public void MotorTorqueSetAndMove(int torque)
        {
            byte[] bSpeed = BitConverter.GetBytes(torque).Reverse().Skip(2).ToArray();
            CmdCopy(bSpeed, MotorSet.torque);
            Thread.Sleep(10);
            MotorSendCommand(cmd_motor_enable.WithCRC16(CRC16Type.Modbus).ToArray()); //转矩启动
        }

        //位移-1073741824 ~1073741824
        /// <summary>
        /// 设置位移脉冲，1000脉冲为1圈，脉冲为负时反转
        /// </summary>
        /// <param name="location"> -1073741824 ~1073741824 </param>
        public void MotorLocTorSetLoc(int location)
        {
            byte[] bSpeed = BitConverter.GetBytes(location).ToArray();
            { //更换0-1、2-3 位置
                byte temp;
                temp = bSpeed[0];
                bSpeed[0] = bSpeed[1];
                bSpeed[1] = temp;

                temp = bSpeed[2];
                bSpeed[2] = bSpeed[3];
                bSpeed[3] = temp;
            }
            CmdCopy(bSpeed, MotorSet.location);
        }

        //1-6000
        /// <summary>
        /// 设置位置模式下转速
        /// </summary>
        /// <param name="speed1"> 转速范围1~6000 </param>
        public void MotorLocSpeed(int speed1)
        {
            byte[] bSpeed = BitConverter.GetBytes(speed1).Reverse().Skip(2).ToArray();
            CmdCopy(bSpeed, MotorSet.locSpeed);
        }

        /// <summary>
        /// 执行设定的脉冲数
        /// </summary>
        public void MotorLocMove()
        {
            MotorSendCommand(cmd_motor_locMove.WithCRC16(CRC16Type.Modbus).ToArray()); //位置到达启动
            MotorSendCommand(cmd_motor_locEnable.WithCRC16(CRC16Type.Modbus).ToArray()); //准备位置到达
        }

        private void CmdCopy(byte[] bspeed, MotorSet motorset)
        {
            byte[] cmd = new byte[cmd_motorMove[motorMode][motorset].Length + bspeed.Length];
            Array.Copy(
                cmd_motorMove[motorMode][motorset],
                cmd,
                cmd_motorMove[motorMode][motorset].Length
            );
            Array.Copy(bspeed, 0, cmd, cmd_motorMove[motorMode][motorset].Length, bspeed.Length);
            MotorSendCommand(cmd.WithCRC16(CRC16Type.Modbus).ToArray());
        }

        private void MotorSendCommand(byte[] cmd)
        {
            StopMonitor = true;
            while (!IsMonitorOK)
            {
                Thread.Sleep(50);
            }
            var res = SendCmdAndWaitResponse(cmd);
            Thread.Sleep(waitTime);
            if (!Check(res))
            {
                throw new Exception($"{deviceName}响应数据CRC校验异常");
            }
            if ((res[1] & 0x80) > 0)
            {
                throw new Exception($"{deviceName}响应异常异常,错误代码[{res[3].ToString()}]");
            }
            Mylog.Debug(
                $"{deviceName}  [参数:{BitConverter.ToString(res.Skip(3).Take(2).ToArray()).Replace("-", " ")}]"
            );
            StopMonitor = false;
        }

        private byte[] MotorSendCommand(byte[] cmd, string name)
        {
            var res = SendCmdAndWaitResponse(cmd);
            if (!Check(res))
            {
                throw new Exception($"{deviceName}响应数据CRC校验异常");
            }
            if ((res[1] & 0x80) > 0)
            {
                throw new Exception($"{deviceName}响应异常异常,错误代码[{res[3].ToString()}]");
            }
            Mylog.Debug(
                $"{deviceName}  [{name}参数:{BitConverter.ToString(res.Skip(3).Take(2).ToArray()).Replace("-", " ")}]"
            );
            return res;
        }

        /// <summary>
        /// 电机实时位置、速度、转矩、监控
        /// </summary>
        /// <exception cref="Exception"> </exception>
        public void MotorMonitor()
        {
            Task.Run(() =>
            {
                while (MonitorStatus)
                {
                    try
                    {
                        if (!StopMonitor)
                        {
                            IsMonitorOK = false;
                            var res = MotorSendCommand(
                                cmd_read_speed.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "速度"
                            );
                            Speed = (short)(res[3] << 8 | res[4]);
                            res = MotorSendCommand(
                                cmd_read_torque.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "转矩"
                            );
                            Torque = (short)(res[3] << 8 | res[4]);
                            res = MotorSendCommand(
                                cmd_read_location.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "位置"
                            );
                            Location = res[5] << 24 | res[6] << 16 | res[3] << 8 | res[4];
                            IsMonitorOK = true;
                        }
                        Thread.Sleep(50);
                    }
                    catch (Exception)
                    {
                        continue;
                        throw new Exception("监控出现异常");
                    }
                }
            });
        }

        /// <summary>
        /// 电机位置监控
        /// </summary>
        /// <exception cref="Exception"> </exception>
        public void MotorMonitorLocation()
        {
            Task.Run(() =>
            {
                while (MonitorLocationStatus)
                {
                    try
                    {
                        if (!StopMonitor)
                        {
                            IsMonitorOK = false;
                            //var res = MotorSendCommand(
                            //    cmd_read_speed.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "速度"
                            //);
                            //Speed = (short)(res[3] << 8 | res[4]);
                            //res = MotorSendCommand(
                            //    cmd_read_torque.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "转矩"
                            //);
                            //Torque = (short)(res[3] << 8 | res[4]);
                            var res = MotorSendCommand(
                                cmd_read_location.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "位置"
                            );
                            Location = res[5] << 24 | res[6] << 16 | res[3] << 8 | res[4];
                            IsMonitorOK = true;
                        }
                        Thread.Sleep(50);
                    }
                    catch (Exception)
                    {
                        continue;
                        throw new Exception("监控出现异常");
                    }
                }
            });
        }

        /// <summary>
        /// 电机速度监控
        /// </summary>
        /// <exception cref="Exception"> </exception>
        public void MotorMonitorSpeed()
        {
            Task.Run(() =>
            {
                while (MonitorSpeedStatus)
                {
                    try
                    {
                        if (!StopMonitor)
                        {
                            IsMonitorOK = false;
                            var res = MotorSendCommand(
                                cmd_read_speed.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "速度"
                            );
                            Speed = (short)(res[3] << 8 | res[4]);
                            //res = MotorSendCommand(
                            //    cmd_read_torque.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "转矩"
                            //);
                            //Torque = (short)(res[3] << 8 | res[4]);
                            //res = MotorSendCommand(
                            //    cmd_read_location.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "位置"
                            //);
                            //Location = res[5] << 24 | res[6] << 16 | res[3] << 8 | res[4];
                            IsMonitorOK = true;
                        }
                        Thread.Sleep(50);
                    }
                    catch (Exception)
                    {
                        continue;
                        throw new Exception("监控出现异常");
                    }
                }
            });
        }

        /// <summary>
        /// 电机转矩监控
        /// </summary>
        /// <exception cref="Exception"> </exception>
        public void MotorMonitorTorque()
        {
            Task.Run(() =>
            {
                while (MonitorTorqueStatus)
                {
                    try
                    {
                        if (!StopMonitor)
                        {
                            IsMonitorOK = false;
                            //var res = MotorSendCommand(
                            //    cmd_read_speed.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "速度"
                            //);
                            //Speed = (short)(res[3] << 8 | res[4]);
                            var res = MotorSendCommand(
                                cmd_read_torque.WithCRC16(CRC16Type.Modbus).ToArray(),
                                "转矩"
                            );
                            Torque = (short)(res[3] << 8 | res[4]);
                            //res = MotorSendCommand(
                            //    cmd_read_location.WithCRC16(CRC16Type.Modbus).ToArray(),
                            //    "位置"
                            //);
                            //Location = res[5] << 24 | res[6] << 16 | res[3] << 8 | res[4];
                            IsMonitorOK = true;
                        }
                        Thread.Sleep(50);
                    }
                    catch (Exception)
                    {
                        continue;
                        throw new Exception("监控出现异常");
                    }
                }
            });
        }

        /// <summary>
        /// 上使能
        /// </summary>
        public void MotorEnable()
        {
            MotorSendCommand(cmd_motor_locEnable.WithCRC16(CRC16Type.Modbus).ToArray()); //上使能
        }

        /// <summary>
        /// 断使能
        /// </summary>
        public void MotorDisable()
        {
            MotorSendCommand(cmd_motor_disable.WithCRC16(CRC16Type.Modbus).ToArray()); //断使能
        }

        /// <summary>
        /// 设置当前位置为原点
        /// </summary>
        public void SetOrigin()
        {
            MotorSendCommand(cmd_set_origin.WithCRC16(CRC16Type.Modbus).ToArray());
        }

        /// <summary>
        /// 回到原点
        /// </summary>
        public void ToOrigin()
        {
            MotorSendCommand(cmd_to_origin.WithCRC16(CRC16Type.Modbus).ToArray());
            MotorSendCommand(cmd_motor_locEnable.WithCRC16(CRC16Type.Modbus).ToArray());
        }

        #endregion 伺服运动指令
    }
}